#include "CTrackMp.h"
#include "CDtlCellDesign.h"
#include "CBeam.h"
#include "CSpaceCharge.h"
#include <sstream>
#include <string>
#include<iostream>
#include<fstream>
#include<iomanip>
#include<cmath>
#include<stdlib.h>
//#include<public.h>
using namespace std;
#define REAL(z,i) ((z)[2*(i)])
#define IMAG(z,i) ((z)[2*(i)+1])
//用于track beam经过一个cell后的束流
//长度单位cm，G单位T／m
//dt的单位是单位s。输入的单位是ns
//注意，beam－>里面的z不是相对于同步粒子的，而是粒子本身的绝对位置
extern double emittance();
CTrackMp::CTrackMp(double _dt)
{
    beam = new CBeam();
    SC = new CSpaceCharge();
		dZedge=0;
    dt=_dt*1.0e-9;
    iOld=0;
}
CTrackMp::~CTrackMp()
{
    delete beam;
    delete SC;
}
CTrackMp::CTrackMp(double _dFreq,double _dt)
{                            
    beam = new CBeam();
    SC = new CSpaceCharge();
    dFreq=_dFreq*1.0e6;
    dWaveLength=clight/dFreq;    //单位m
    dt=_dt*1.0e-9;
		dZedge=0;
    beam->setFreq(dFreq);
    iOld=0;
}
void CTrackMp::SetFreq(double _dFreq)
{
    dFreq=_dFreq*1.0e6;
    dWaveLength=clight/dFreq;
    beam->setFreq(dFreq);
}
void CTrackMp::SetDt(double _dt)    //单位是ns
{
    //    dt=_dt*1.0e-9;
    beam->setStep(_dt*1.0e-9);
}
void CTrackMp::GetParticleNum()    //depending!!!!
{
    iParticleNumber=beam->getParticleNumber();
} 
//void CTrackMp::SetBeam(CBeam &_beam)
//{
//  beam=_beam;
//  iParticleNumber=beam->getParticleNumber();
//}
/*
   void CTrackMp::moveOneStep(double *_t0)
   {            
   t0=_t0;
   iParticleNumber=beam->getParticleNumber();
   for(int iTmp=1;iTmp<=iParticleNumber;iTmp++)
   {
   dX=beam->p.at(iTmp).getPosition().getX();
   dY=beam->p.at(iTmp).getPosition().getY();
   dZ=beam->p.at(iTmp).getPosition().getZ();
   beam->p.at(iTmp).setE(dEx,dEy,dEz);
   beam->p.at(iTmp).setB(dBx*dY,dBy*dX,dBz*dZ);//需要改改 ,假如得到的dx等单位是m
   beam->p.at(iTmp).particle::moveOneStep(*t0);
   }
   }
 */
double CTrackMp::XI0(double x)
    //	xi0 为 Io(x), 即零阶虚宗量Bessel函数
{
    double y, Z;
    y = fabs(x);
    if(y != 3.75)
    {
        Z=x*x*7.111111e-2;
        y=((((( 4.5813e-3*Z+3.60768e-2)*Z+2.659732e-1)*Z+1.206749e0)*Z
                    +3.089942e0)*Z+3.515623e0)*Z+1.0;
    }
    else
    {
        Z=3.75/y;
        y = exp(y)/sqrt(y)*((((((((3.92377e-3*Z-1.647633e-2)*Z
                                        +2.635537e-2)*Z-2.057706e-2)*Z+9.16281e-3)*Z-1.57565e-3)*Z
                        +2.25319e-3)*Z+1.328592e-2)*Z+3.989423e-1);
    }

    return (y);
}
double CTrackMp::XI1(double X)
{	
    double y, A, B, C, K;
    A=0.5*X;
    y=A;
    B=A;
    K=1.0;
G100: B=B*A*A/(K*(K+1.0));
      C=B;
      y=y+C;
      K=K+1;
      if(C>0.00000001)goto G100;
      return (y);
}
void CTrackMp::SetSfline(vector<double> *_sfline)
{
    dBetac=_sfline->at(0);
    dT=_sfline->at(1);
    dTp=_sfline->at(2);
    dGammac=sqrt(1/(1-dBetac/dBetac));
}
double  CTrackMp::Drift_Matrix(double _dL)
{
    //getBeam();
    double dtTmp;
    double dv=beam->getBetaZAve()*clight;
    dtTmp=_dL/100.0/dv;
    double dvxTmp,dvyTmp,dvzTmp;
    double dxTmp,dyTmp,dzTmp;
    double dt0;
		iParticleNumber=beam->getParticleNumber();
    for(int iTmp=0;iTmp<=iParticleNumber;iTmp++)
    {
        dvxTmp=beam->p.at(iTmp).getVelocity().getX();
        dvyTmp=beam->p.at(iTmp).getVelocity().getY();
        dvzTmp=beam->p.at(iTmp).getVelocity().getZ();
        dxTmp=beam->p.at(iTmp).getPosition().getX();
        dyTmp=beam->p.at(iTmp).getPosition().getY();
        dzTmp=beam->p.at(iTmp).getPosition().getZ();
        dt0=beam->p.at(iTmp).getT0();
        beam->p.at(iTmp).setT0(dt0+dtTmp);
        dxTmp+=dvxTmp*dtTmp;
        dyTmp+=dvyTmp*dtTmp;
        dzTmp+=dvzTmp*dtTmp;
        beam->p.at(iTmp).setPosition(dxTmp,dyTmp,dzTmp);
    }
    getBeam();
		dZedge+=_dL;
}
void CTrackMp::Drift(double _dL)  //_dL is in cm
{
    double dL=_dL/100.0;
    double dz=beam->p.at(0).getPosition().getZ();
    double dDeltz=0;
    double dtTmp=dt;
    double dVz;
		iParticleNumber=beam->getParticleNumber();
    for(int iTmp=0;iTmp<=iParticleNumber;iTmp++)
    {
        dDeltz=0;
        while(dDeltz<dL-dZero)
        {
            dZ=beam->p.at(iTmp).getPosition().getZ();
            dVz=beam->p.at(iTmp).getVelocity().getZ();
            if(dDeltz+dVz*dtTmp>=dL)
            {
                dtTmp=(dL-dDeltz)/dVz;
                SetDt(dtTmp*1.0e9);
            }
            beam->p.at(iTmp).setE(0,0,0);
            beam->p.at(iTmp).setB(0,0,0);
            beam->p.at(iTmp).particle::moveOneStep();
            if(dDeltz+dVz*dtTmp>=dL)     { SetDt(dt*1.0e9);}     //dt的问题是不是出在这？
            dDeltz+=(beam->p.at(iTmp).getPosition().getZ()-dZ);
        }
    }
    beam->p.at(0).setPositionZ(dz+dL);
    getBeam();
		dZedge+=dL;
}
void CTrackMp::Quad_Matrix(double dQL,double dQS)
{
    double dtTmp;
    double dk;
    double dvz=clight*beam->getBetaZAve();
    double dt0;
    double dvxTmp,dvyTmp,dvzTmp;
    double dxTmp,dyTmp,dzTmp;
    double dGamma;
    double d1,d2,d3,d4;
    dtTmp=dQL/100.0/dvz;
		iParticleNumber=beam->getParticleNumber();
    for(int iTmp=0;iTmp<=iParticleNumber;iTmp++)
    {
        dvxTmp=beam->p.at(iTmp).getVelocity().getX();
        dvyTmp=beam->p.at(iTmp).getVelocity().getY();
        dvzTmp=beam->p.at(iTmp).getVelocity().getZ();
        dxTmp=beam->p.at(iTmp).getPosition().getX();
        dyTmp=beam->p.at(iTmp).getPosition().getY();
        dzTmp=beam->p.at(iTmp).getPosition().getZ();
        dt0=beam->p.at(iTmp).getT0();
        beam->p.at(iTmp).setT0(dt0+dtTmp);
        //dGamma=1.0/sqrt(1.0-(dvxTmp*dvxTmp+dvyTmp*dvyTmp+dvzTmp*dvzTmp)/clight/clight);
        dk=sqrt(1.0-1.0/dGamma/dGamma)*fabs(dQS)/dGamma/dErest/1.0e6*clight*clight;
        dk=dvzTmp*fabs(dQS)/dGamma/dErest/1.0e6*clight*clight;
        if(dQS>0)
        {d1=cos(sqrt(dk)*dtTmp);d2=sin(sqrt(dk)*dtTmp);d3=cosh(sqrt(dk)*dtTmp);d4=sinh(sqrt(dk)*dtTmp);}
        else
        {d3=cos(sqrt(dk)*dtTmp);d4=sin(sqrt(dk)*dtTmp);d1=cosh(sqrt(dk)*dtTmp);d2=sinh(sqrt(dk)*dtTmp);}
        dxTmp=dxTmp*d1+dvxTmp*d2/sqrt(dk);
        dyTmp=dyTmp*d3+dvyTmp*d4/sqrt(dk);
        dvxTmp=-sqrt(dk)*dxTmp*d2+dvxTmp*d1;
        dvyTmp=-sqrt(dk)*dyTmp*d4+dvyTmp*d3;
        dzTmp+=dvzTmp*dtTmp;
        beam->p.at(iTmp).setPosition(dxTmp,dyTmp,dzTmp);
        beam->p.at(iTmp).setVelocity(dvxTmp,dvyTmp,dvzTmp);
    }
    getBeam();
    dZedge+=dQL;
}
void CTrackMp::Quad(double dQL,double dQS)
{
    dQL=dQL/100.0;
    getBeam();
    double dDeltz=0;
    double dtTmp=dt;
    double dX,dY,dZ,dVz;
    CVector3d *EOut;
    CVector3d *BOut;
    EOut=new CVector3d();
    BOut=new CVector3d();
    double dBx,dBy,dBz;
    double dEx,dEy,dEz;
    double dz=beam->p.at(0).getPosition().getZ();
    for(int iTmp=0;iTmp<=iParticleNumber;iTmp++)
    {
        dDeltz=0;
        while(dDeltz<dQL-dZero)
        {
            dZ=beam->p.at(iTmp).getPosition().getZ();
            dX=beam->p.at(iTmp).getPosition().getX();
            dY=beam->p.at(iTmp).getPosition().getY();
            dVz=beam->p.at(iTmp).getVelocity().getZ();
            Interpret(dX,dY,dZ,EOut,BOut);
            dEx=EOut->getX();dEy=EOut->getY();dEz=EOut->getZ();
            dBx=BOut->getX()+dQS*dY; dBy=BOut->getY()+dQS*dX;dBz=BOut->getZ();
            if(dDeltz+dVz*dtTmp>=dQL)
            {
                dtTmp=(dQL-dDeltz)/dVz;
                SetDt(dtTmp*1.0e9);
            }
            if(dDeltz<0||dDeltz>dQL) {dBx=BOut->getX(); dBy=BOut->getY();dBz=BOut->getZ();}
            beam->p.at(iTmp).setE(dEx,dEy,dEz);
            beam->p.at(iTmp).setB(dBx,dBy,0);//需要改改 ,假如得到的dx等单位是m
            beam->p.at(iTmp).particle::moveOneStep();
            if(dDeltz+dVz*dtTmp>=dQL)     { SetDt(dt*1.0e9);}
            dDeltz+=(beam->p.at(iTmp).getPosition().getZ()-dZ);
        }
    }
    beam->p.at(0).setPositionZ(dz+dQL);
    getBeam();
    delete EOut;
    delete BOut;
    dZedge+=dQL;
}
void CTrackMp::ThinGap(double _dEnergyS,double _dE0TL, double _phi, double _dFreqThinGap, double _aperture)
{
    /*
       double dphi,dE0TL,dFreqThinGap,dW;
       double dZAve,dZpAve;
       double dGammaS,dGammaE,dBetaE;
       dZpAve=beam->getBetaS();
       dZAve=beam->p.at(0).getPosition().getZ();
       dE0TL=_dE0TL;
       dphi=_phi*pi/180.0;
       dAperture=_aperture;
       dFreqThinGap=_dFreqThinGap;       //freq 是不是会有什么影响？？？？
       dW=beam->p.at(0).getVelocity().getZ()+dE0TL*cos(dphi);
       beam->p.at(0).setVelocity(0,0,dW);
       dGammaS=1.0/sqrt(1.0-dZpAve*dZpAve);
       dGammaE=dW/dErest+1.0;
       double dConst;
       dConst=dE0TL*1.0e6*pi/dZpAve/dZpAve/clight*dFreqThinGap/dErest/dGammaS/dGammaS*sin(dphi);
       double dphiTmp,dETmp;
       double dzTmp,dzpTmp,dGammaETmp,dxTmp,dyTmp,dvxTmp,dvyTmp;
       for(int i=1;i<=iParticleNumber;i++)
       {
       dzpTmp=beam->p.at(i).getVelocity().getZ();
       dzTmp=beam->p.at(i).getPosition().getZ();
       dphiTmp=2*pi*dFreqThinGap*1.0e6*(dzTmp-dZAve)/dZpAve/clight+dphi;
       dETmp=dErest/sqrt(1-dzpTmp*dzpTmp/clight/clight)+dE0TL*cos(dphiTmp);
    //    dETmp=dErest/sqrt(1-dzpTmp*dzpTmp/clight/clight)+dE0TL*cos(dphi);
    dGammaETmp=dETmp/dErest;
    dzpTmp=clight*sqrt(1-1/dGammaETmp/dGammaETmp);
    dxTmp=beam->p.at(i).getPosition().getX();
    dyTmp=beam->p.at(i).getPosition().getY();
    dvxTmp=beam->p.at(i).getVelocity().getX();
    dvyTmp=beam->p.at(i).getVelocity().getY();
    dvxTmp=clight*(-dConst*(dxTmp)+dGammaS*dvxTmp/clight)/dGammaE;
    dvyTmp=clight*(-dConst*(dyTmp)+dGammaS*dvyTmp/clight)/dGammaE;
    beam->p.at(i).setVelocity(dvxTmp,dvyTmp,dzpTmp);
    }
     */

    double dPhiS=_phi*pi/180;
    double dEnergyS=_dEnergyS;
    double dGammaS=dEnergyS/dErest+1.0;
    double dBetaS=sqrt(1.0-1.0/dGammaS/dGammaS);
    double dEnergyES=dEnergyS+_dE0TL*cos(dPhiS);
    double dGammaES=dEnergyS/dErest+1.0;
    double dBetaES=sqrt(1.0-1.0/dGammaS/dGammaS);
    if(_dE0TL>1e-5)
    {
        double dPhiP;
        double dxP,dyP,dzP,dvxP,dvyP,dvzP,dzAve;
        double dxpP,dypP,dzpP;
        dzAve=beam->p.at(0).getPosition().getZ();
        double dConst1;
        dConst1=-dE0*dCellLength/dErest*1.0e-6;
        double dkP,dKP,dRP;
        double dDeltPhiP;
        double dEnergyP,dGammaP,dBetaP;
        double dEnergyEP,dGammaEP,dBetaEP;
        double dBetaAve,dGammaAve;
        double dDeltW;
        //  double dPhiAve;
        //  dPhiAve=0;
        vector<double> *vec;


        //==========from parmila==================
        ofstream ofP("parmila.txt");
        for(int i=0;i<=iParticleNumber;i++)
        {
            dxP=beam->p.at(i).getPosition().getX();
            dyP=beam->p.at(i).getPosition().getY();
            dzP=beam->p.at(i).getPosition().getZ();
            dvxP=beam->p.at(i).getVelocity().getX();
            dvyP=beam->p.at(i).getVelocity().getY();
            dvzP=beam->p.at(i).getVelocity().getZ();
            dPhiP=-(dzP)/dvzP*dFreq*2*pi;
            dEnergyP=dErest/sqrt(1.0-(dvxP*dvxP+dvyP*dvyP+dvzP*dvzP)/clight/clight)-dErest;
            ofP<<setprecision(15)<<dxP<<" " <<dvxP/dBetaS/clight<<" " <<dyP<<" " <<dvyP/dBetaS/clight<<" " <<dPhiP<<" " <<dEnergyP<<endl;
        }
        ofP.close();
        for(int i=0;i<=iParticleNumber;i++)
        {
            dxP=beam->p.at(i).getPosition().getX();
            dyP=beam->p.at(i).getPosition().getY();
            dzP=beam->p.at(i).getPosition().getZ();
            dvxP=beam->p.at(i).getVelocity().getX();
            dvyP=beam->p.at(i).getVelocity().getY();
            dvzP=beam->p.at(i).getVelocity().getZ();
            dPhiP=-(dzP)/dvzP*_dFreqThinGap*1.0e6*2*pi+dPhiS;

            dEnergyP=dErest/sqrt(1.0-(dvxP*dvxP+dvyP*dvyP+dvzP*dvzP)/clight/clight)-dErest;
            dGammaP=dEnergyP/dErest+1.0;
            dBetaP=sqrt(1.0-1.0/dGammaP/dGammaP);
            // dvxP*=dGammaP;
            // dvyP*=dGammaP;
            // dvzP*=dGammaP;
            dDeltW=_dE0TL*cos(dPhiP);
            dEnergyEP=dEnergyP+dDeltW;
            dGammaEP=dEnergyEP/dErest+1.0;
            dBetaEP=sqrt(1.0-1.0/dGammaEP/dGammaEP);
            dBetaAve=(dBetaP+dBetaEP)/2.0;
            dGammaAve=(dGammaP+dGammaEP)/2.0;
            double dkx=-pi*_dE0TL*sin(dPhiP)/dErest/dBetaAve/dBetaAve/dGammaAve/dGammaAve/clight*_dFreqThinGap*1.0e6;
            double dky=-pi*_dE0TL*sin(dPhiP)/dErest/dBetaAve/dBetaAve/dGammaAve/dGammaAve/clight*_dFreqThinGap*1.0e6;
            double dkz=2*pi*_dE0TL*sin(dPhiP)/dErest/dBetaAve/dBetaAve/clight*_dFreqThinGap*(dzP-dzAve); //z是什么？
            double dBGRatio=dBetaP*dGammaP/dBetaEP/dGammaEP;
            dxpP=dvxP/dBetaS/clight;
            dypP=dvyP/dBetaS/clight;
            dxpP=dxpP*dBGRatio+dkx*dxP/dBetaEP/dGammaEP;
            dypP=dypP*dBGRatio+dky*dyP/dBetaEP/dGammaEP;
            dvxP=dxpP*(dBetaES)*clight;
            dvyP=dypP*(dBetaES)*clight;
            dvzP=clight*sqrt(1.0-1.0/(dEnergyEP/dErest+1.0)/(dEnergyEP/dErest+1.0));
            dvzP=sqrt(dvzP*dvzP-dvxP*dvxP-dvyP*dvyP);

            beam->p.at(i).setVelocity(dvxP,dvyP,dvzP);
            beam->p.at(i).setPositionZ(-(dPhiP-dPhiS)*dvzP/2/pi/_dFreqThinGap*1.0e-6);
        }
        ofP.open("parmila-2.txt");
        for(int i=0;i<=iParticleNumber;i++)
        {
            dxP=beam->p.at(i).getPosition().getX();
            dyP=beam->p.at(i).getPosition().getY();
            dzP=beam->p.at(i).getPosition().getZ();
            dvxP=beam->p.at(i).getVelocity().getX();
            dvyP=beam->p.at(i).getVelocity().getY();
            dvzP=beam->p.at(i).getVelocity().getZ();
            dPhiP=-(dzP)/dvzP*_dFreqThinGap*1.0e6*2*pi;
            dEnergyP=dErest/sqrt(1.0-(dvxP*dvxP+dvyP*dvyP+dvzP*dvzP)/clight/clight)-dErest;
            ofP<<setprecision(15)<<dxP<<" " <<dvxP/dBetaES/clight<<" " <<dyP<<" " <<dvyP/dBetaES/clight<<" " <<dPhiP<<" " <<dEnergyP<<endl;
        }
        ofP.close();
    }

    /*
       double dPhiS=_phi*pi/180;
       double dEnergyS=_dEnergyS;
       double dGammaS=dEnergyS/dErest+1.0;
       double dBetaS=sqrt(1.0-1.0/dGammaS/dGammaS);
       double dEnergyES=dEnergyS+_dE0TL*cos(dPhiS);
       double dGammaES=dEnergyS/dErest+1.0;
       double dBetaES=sqrt(1.0-1.0/dGammaS/dGammaS);
       double dPhiP;
       double dxP,dyP,dzP,dvxP,dvyP,dvzP,dzAve;
       double dxpP,dypP,dzpP;
       dzAve=beam->p.at(0).getPosition().getZ();
       double dConst1;
       dConst1=-dE0*dCellLength/dErest*1.0e-6;
       double dkP,dKP,dRP;
       double dDeltPhiP;
       double dEnergyP,dGammaP,dBetaP;
       double dEnergyEP,dGammaEP,dBetaEP;
       double dBetaAve,dGammaAve;
    //  double dPhiAve;
    //  dPhiAve=0;
    vector<double> *vec;
    ofstream ofP("parmila.txt");
    for(int i=1;i<=iParticleNumber;i++)
    {
    dxP=beam->p.at(i).getPosition().getX();
    dyP=beam->p.at(i).getPosition().getY();
    dzP=beam->p.at(i).getPosition().getZ();
    dvxP=beam->p.at(i).getVelocity().getX();
    dvyP=beam->p.at(i).getVelocity().getY();
    dvzP=beam->p.at(i).getVelocity().getZ();
    dPhiP=-(dzP)/dvzP*dFreq*2*pi;
    dEnergyP=dErest/sqrt(1.0-(dvxP*dvxP+dvyP*dvyP+dvzP*dvzP)/clight/clight)-dErest;
    ofP<<setw(8)<<dxP<<" " <<dvxP/dBetaS/clight<<" " <<dyP<<" " <<dvyP/dBetaS/clight<<" " <<dPhiP<<" " <<dEnergyP<<endl;
    }
    ofP.close();
    dTTF=1.0;dTTFp=0,dSTF=0;dSTFp=0;
    dBetag=(dBetaS+dBetaES)/2.0;
    double dGammag=(dGammaS+dGammaES)/2.0;
    dCellLength=dBetag*dGammag;

    double dConst2=pi/dErest*_dE0TL/pow((dBetag*dGammag),3)/clight*dFreq;
    double dConst3=dTTF*pow((pi/dGammag/dBetag/clight*dFreq),2);
    double dConst4=dTTF/dBetag/clight*dFreq*pi;
    double dConst5=0.5*_dE0TL*dTTF/dErest/dBetag/dBetag/dGammag*(dTTF+dTTFp/dGammag/dGammag);
    double dConst0=dConst2*dCellLength;  //dConst1 in parmila
    double dDeltV;
    double dTW;
    double dRSqureP;
    double dRRpP;
    double dDeltW;
    double dBGRatio;
    double dDOverL;
    double OneMinusA;
    double dPhiPAve=0;
    double dPhiTAve=0;
    for(int i=1;i<=iParticleNumber;i++)
    {
    dxP=beam->p.at(i).getPosition().getX();
    dyP=beam->p.at(i).getPosition().getY();
    dzP=beam->p.at(i).getPosition().getZ();
    dvxP=beam->p.at(i).getVelocity().getX();
    dvyP=beam->p.at(i).getVelocity().getY();
    dvzP=beam->p.at(i).getVelocity().getZ();
    dPhiP=-(dzP)/dvzP*_dFreqThinGap*1.0e6*2*pi;
    //dPhiP=(dzP-dzAve)/dvzP*dFreq*2*pi-(dvzP-dBeta*clight)/dvzP*pi;
    //dPhiP=dzP/dBeta/clight*dFreq*2*pi-(dvzP-dBeta*clight)/dvzP*pi;
    dxpP=dvxP/dBetaS/clight;
    dypP=dvyP/dBetaS/clight;
    // dxpP=dvxP/dvzP;
    // dypP=dvyP/dvzP;

    double dPhiT;
    double dProme1=-dConst0*(dTTFp/(-2)/pi*(sin(dPhiP)-sin(dPhiS))+dSTFp*(cos(dPhiP)-cos(dPhiS)));
    dPhiT=dPhiP+dProme1+dPhiS;

    dPhiPAve+=dPhiP;
    dPhiTAve+=dPhiT;
    dEnergyP=dErest/sqrt(1.0-(dvxP*dvxP+dvyP*dvyP+dvzP*dvzP)/clight/clight)-dErest;
    dGammaP=dEnergyP/dErest+1.0;
    dBetaP=sqrt(1.0-1.0/dGammaP/dGammaP);

    dDeltV=dBetaS/dvzP*clight-1.0;
    dTW=dTTF+dTTFp*dDeltV;
    dRSqureP=(dxP*dxP+dyP*dyP);
    dRRpP=dxP*dxpP+dyP*dypP;
    dDeltW=_dE0TL*((dTW+dConst3*dRSqureP)*cos(dPhiT)-dRRpP*dConst4*sin(dPhiT));
    dEnergyEP=dEnergyP+dDeltW;
    dGammaEP=dEnergyEP/dErest+1.0;
    dBetaEP=sqrt(1.0-1.0/dGammaEP/dGammaEP);

    dBGRatio=dvzP/clight*dGammaP/dBetaEP/dGammaEP;      
    dDOverL=dConst2*dTW*sin(dPhiT);
    OneMinusA=1.0-dConst5*cos(dPhiT);
    double dProme;
    dProme=dConst0*dTTFp/pi*(sin(dPhiT)-sin(dPhiS));
    dPhiP=dPhiP+dProme;
    //dPhiP=dPhiP-(dBetaEP-dBetaES)/dBetaEP*pi+dProme;

    //OneMinusA=1.0;
    dxpP=-dDOverL*dxP+dBGRatio/OneMinusA*dxpP;
    dypP=-dDOverL*dyP+dBGRatio/OneMinusA*dypP;
    dvxP=dxpP*dBetaES*clight;
    dvyP=dypP*dBetaES*clight;
    dvzP=sqrt((1.0-1.0/(dGammaEP*dGammaEP))*clight*clight-dvxP*dvxP-dvyP*dvyP);
    //  dvzP=clight*sqrt(1.0-1.0/dGammaEP/dGammaEP) /(1.0+dxpP*dxpP+dypP*dypP);
    //  dvxP=dxpP*dvzP;
    //  dvyP=dypP*dvzP;

    beam->p.at(i).setVelocityX(dvxP);
    beam->p.at(i).setVelocityY(dvyP);
    beam->p.at(i).setVelocityZ(dvzP);
    //beam->p.at(i).setVelocityX((-dDOverL*dxP+dBGRatio/OneMinusA*dxpP)*dBetaES*clight);
    //beam->p.at(i).setVelocityY((-dDOverL*dyP+dBGRatio/OneMinusA*dypP)*dBetaES*clight);
    //  beam->p.at(i).setPositionX(dxP*OneMinusA);
    //  beam->p.at(i).setPositionY(dyP*OneMinusA);
    //  beam->p.at(i).setPositionZ(-dPhiP/2/pi*dvzP/dFreq);
    // beam->p.at(i).setPositionZ(dPhiP/2/pi*dBetaES*clight/dFreq);

} 
ofP.open("parmila-2.txt");
for(int i=1;i<=iParticleNumber;i++)
{
    dxP=beam->p.at(i).getPosition().getX();
    dyP=beam->p.at(i).getPosition().getY();
    dzP=beam->p.at(i).getPosition().getZ();
    dvxP=beam->p.at(i).getVelocity().getX();
    dvyP=beam->p.at(i).getVelocity().getY();
    dvzP=beam->p.at(i).getVelocity().getZ();
    dPhiP=-(dzP)/dvzP*dFreq*2*pi;
    dEnergyP=dErest/sqrt(1.0-(dvxP*dvxP+dvyP*dvyP+dvzP*dvzP)/clight/clight)-dErest;
    ofP<<setw(8)<<dxP<<" " <<dvxP/dBetaES/clight<<" " <<dyP<<" " <<dvyP/dBetaES/clight<<" " <<dPhiP<<" " <<dEnergyP<<endl;
}
ofP.close();
*/
}
void CTrackMp::SetTTF(double _dT,double _dTp,double _dS,double _dSp)
{
    dTTF=_dT;
    dTTFp=_dTp*(-2)*pi;
    dSTF=_dS;
    dSTF=_dSp;
}
void CTrackMp::SetPhaseMove(double _Phase)
{
    iParticleNumber=beam->getParticleNumber();
    dPhaseChange=_Phase;
    double dT0;
		double dX0,dY0,dZ0;
		double dVX0,dVY0,dVZ0;
		double dZS;
		dT0=dPhaseChange/360.0/dFreq;
    for(int i=0;i<=iParticleNumber;i++)
    {
        //beam->vdPhase.at(i)+=_Phase*pi/180.0;
        dT0=beam->p.at(i).getT0();
        beam->p.at(i).setT0(dT0+dPhaseChange/360.0/dFreq);  //前提是t0的单位是s，注意拉！！！！！还有正负号
				//dX0=beam->p.at(i).getPosition().getX();
				//dY0=beam->p.at(i).getPosition().getY();
				//dZ0=beam->p.at(i).getPosition().getZ();
				//dVX0=beam->p.at(i).getPosition().getX();
				//dVY0=beam->p.at(i).getPosition().getY();
				//dVZ0=beam->p.at(i).getPosition().getZ();
				//dX0=dX0-dT0*dVX0;
				//dY0=dY0-dT0*dVY0;
				//dZ0=dZ0-dT0*dVZ0;
				//beam->p.at(i).getPosition().setX(dX0);
				//beam->p.at(i).getPosition().setY(dY0);
				//beam->p.at(i).getPosition().setZ(dZ0);
    }
}
void CTrackMp::DtlCell(double _dLeft,double _dRight,double _dGap,double _dE0,
        double _dPhis,double _dLQuad1,double _dSQuad1,double _dLQuad2,double _dSQuad2,
        double _dEnergyi,double _dEnergyf,int _itest)
//dLeft,dRight means the left and right cell length
{
    iParticleNumber=beam->getParticleNumber();
    dLeft=_dLeft/100;dRight=_dRight/100;dE0=_dE0*1.0e6;
    dPhiS=_dPhis*pi/180;
    dLQuad1=_dLQuad1/100;dSQuad1=_dSQuad1;dLQuad2=_dLQuad2/100;dSQuad2=_dSQuad2;
    dGap=_dGap/100;
    dCellLength=dLeft+dRight;    
    dBetag=dCellLength*dFreq/clight;
    double dGammag=1.0/sqrt(1.0-dBetag*dBetag);
    dEnergyi=_dEnergyi;
    dEnergyES=_dEnergyf;
    dEnergyS=dEnergyi;
    if(dEnergyES!=0)      
    {
        dGammaES=dEnergyES/dErest+1;
        dBetaES=sqrt(1-1/dGammaES/dGammaES);
    }
    dGammai=dEnergyS/dErest+1;
    dBetai=sqrt(1-1/dGammai/dGammai);
    dGammaS=dGammai;
    dBetaS=dBetai;

    //    dPhiS=dPhiS+2*pi*dBetaES/(dBetai+dBetaES);
    double dtTmp;
    dBeta=dBetai;
    //这个函数里面的磁场是指在1cm参考半径的磁场值
    //测试＝＝＝＝＝＝＝＝＝＝＝＝
    //================================
# if 0	
    Quad(dLQuad1*100,dSQuad1);
    Drift((dLeft-dLQuad1)*100.0);
    if(dE0>dZero)
    {
        double dPChange=0;
        for(int i=0;i<=iParticleNumber;i++)
        {
            double dPhiP;
            double dxP,dyP,dzP,dvxP,dvyP,dvzP;

            //  cDtlCell->SetSfline(dBetag);            //应该用betas还是betag？？？？
            //  vec=cDtlCell->GetSfline();
            //  dTTF=(*vec)[1]; dTTFp=(*vec)[2]*(-2)*pi;dSTF=(*vec)[3];dSTFp=(*vec)[4];

            //==========from parmila==================
            double dEnergyP,dGammaP,dBetaP;
            double dxpP,dypP,dzpP;
            double dConst2=pi/dErest*dE0*1.0e-6*dCellLength/pow((dBetag*dGammag),3)/clight*dFreq;
            double dConst3=dTTF*pow((pi/dGammag/dBetag/clight*dFreq),2);
            double dConst4=dTTF/dBetag/clight*dFreq*pi;
            double dConst5=0.5*dE0*1.0e-6*dCellLength*dTTF/dErest/dBetag/dBetag/dGammag*(dTTF+dTTFp/dGammag/dGammag);
            double dConst0=dConst2*dCellLength;  //dConst1 in parmila
            double dDeltV;
            double dTW;
            double dRSqureP;
            double dRRpP;
            double dDeltW;
            double dEnergyEP;
            double dGammaEP;
            double dBetaEP;
            double dBGRatio;
            double dDOverL;
            double OneMinusA;
            double dPhiT;
            double dProme1;
            dxP=beam->p.at(i).getPosition().getX();
            dyP=beam->p.at(i).getPosition().getY();
            dzP=beam->p.at(i).getPosition().getZ();
            dvxP=beam->p.at(i).getVelocity().getX();
            dvyP=beam->p.at(i).getVelocity().getY();
            dvzP=beam->p.at(i).getVelocity().getZ();
            //==========================================
            //change cordinate to which position is independent variable
            // tmp<<dxP<<" "<<dyP<<endl;
            //dPhiP=-(dzP-dzAve)/dvzP*dFreq*2*pi+dPhaseChange*pi/180;
            //dPhiP=beam->vdPhase.at(i)-(dvzP-dBeta*clight)/dvzP*pi;
            dPhiP=(beam->p.at(i).getT0())*2*pi*dFreq;
            if(i==0&&dPhiP!=dPhiS)
            {
                dPChange=dPhiP-dPhiS;
            }
            dPhiP=dPhiP+dPChange;
            if(i==0) cout<<"dPhiP is "<<dPhiP*180/pi<<endl;
            //==========================================
            dxpP=dvxP/dvzP;
            dypP=dvyP/dvzP;

            dProme1=-dConst0*(dTTFp/(-2)/pi*(sin(dPhiP)-sin(dPhiS))+dSTFp*(cos(dPhiP)-cos(dPhiS)));
            dPhiT=dPhiP+dProme1;

            dBetaP=sqrt(dvzP*dvzP+dvxP*dvxP+dvyP*dvyP)/clight; //对应总的能量
            dGammaP=1.0/sqrt(1.0-dBetaP*dBetaP);
            dEnergyP=(dGammaP-1.0)*dErest;

            dDeltV=dBeta/dvzP*clight-1.0;                      //和总速度对应？
            dTW=dTTF+dTTFp*dDeltV;
            dRSqureP=dxP*dxP+dyP*dyP;
            dRRpP=dxP*dxpP+dyP*dypP;
            //dDeltW=dE0*1.0e-6*dCellLength*((dTW+dConst3*dRSqureP)*cos(dPhiT));
            dDeltW=dE0*1.0e-6*dCellLength*((dTW+dConst3*dRSqureP)*cos(dPhiT)+dRRpP*dConst4*sin(dPhiT));
            dEnergyEP=dEnergyP+dDeltW;
            dGammaEP=dEnergyEP/dErest+1.0;
            dBetaEP=sqrt(1.0-1.0/dGammaEP/dGammaEP);

            dBGRatio=dBetaP*dGammaP/dBetaEP/dGammaEP;      
            dDOverL=dConst2*dTW*sin(dPhiT);
            OneMinusA=1.0-dConst5*cos(dPhiT);
            // dvzP=dBetaEP*clight;
            double dProme;
            dProme=dConst0*dTTFp/pi*(sin(dPhiT)-sin(dPhiS));
            dPhiP=dPhiP+dProme;
            //  dzP=-(dPhiP-dPhaseChange*pi/180.0)/2/pi*dvzP/dFreq+dzAve;
            dzP-=dProme/2/pi*dvzP/dFreq;
            // dPhiP+=dConst0*dTTFp/pi*(-sin(dPhiS));

            dxpP=-dDOverL*dxP+dBGRatio/OneMinusA*dxpP;
            dypP=-dDOverL*dyP+dBGRatio/OneMinusA*dypP;
            dvzP=dBetaEP/sqrt(1+dxpP*dxpP+dypP*dypP)*clight;
            dvxP=dxpP*dvzP;
            dvyP=dypP*dvzP;
            dxP=dxP*OneMinusA;
            dyP=dyP*OneMinusA;
            beam->p.at(i).setT0(dPhiP/2/pi/dFreq);
            //beam->vdPhase.at(i)=dPhiP-(dvzP-dBetaES*clight)/dvzP*pi;

            //=================================
            //cordinate change back
            //	dxP=dxP+dxpP*(dzP-dzAve);
            //	dyP=dyP+dypP*(dzP-dzAve);
            //=================================
            beam->p.at(i).setVelocity(dvxP,dvyP,dvzP);
            beam->p.at(i).setPosition(dxP,dyP,dzP);

            // dzAve-=dConst0*dTTFp/pi*sin(dPhiS)/2/pi*dBetaES/clight/dFreq;
            dBeta=dBetaES;
            beam->p.at(0).setVelocityZ(dBetaES*clight);
            //beam->p.at(0).setVelocityZ(dEnergyES);
            //=========end from parmila==============
        }
    }
    Drift((dRight-dLQuad2)*100.0);
    Quad(dLQuad2*100.0,dSQuad2);
# endif
# if 1
    // GapPhase(); 
    //  getBeam();
    //    ofstream OfField("Ez-In-Axies");
    double dPhi=0;
    double dDeltz=0;
    double dX,dY,dZ,dVz;
    CVector3d *EOut;                    //声明为什么不能挪到.h函数中呢？
    CVector3d *BOut;
    EOut=new CVector3d();
    BOut=new CVector3d();
    double dz=beam->p.at(0).getPosition().getZ();
    //begin
    /* 
       for(int iTmp=0;iTmp<=iParticleNumber;iTmp++)
       {
       dDeltz=0;
       while(dDeltz<dCellLength-dZero)
       {
       dPhi=dFreq*2*pi*(beam->p.at(iTmp).getT0());
       dZ=beam->p.at(iTmp).getPosition().getZ();
       dX=beam->p.at(iTmp).getPosition().getX();
       dY=beam->p.at(iTmp).getPosition().getY();
       dVz=beam->p.at(iTmp).getVelocity().getZ();
       Interpret(dX,dY,dZ,EOut,BOut);
    //dEx=EOut->getX();dEy=EOut->getY();dEz=EOut->getZ();
    dBx=BOut->getX()*(-sin(dPhi));dBy=BOut->getY()*(-sin(dPhi));dBz=BOut->getZ();  //dBz=0
    dEx=EOut->getX()*(cos(dPhi));dEy=EOut->getY()*(cos(dPhi));dEz=EOut->getZ()*cos(dPhi);

    if(dDeltz<dLQuad1) { dBx=BOut->getX()+dSQuad1*dY;dBy=BOut->getY()+dSQuad1*dX;dBz=BOut->getZ(); }
    else if(dDeltz>dCellLength-dLQuad2) { dBx=BOut->getX()+dSQuad2*dY;dBy=BOut->getY()+dSQuad2*dX;dBz=BOut->getZ(); }
    else {dBx=BOut->getX();dBy=BOut->getY();dBz=BOut->getZ();}
    if(dDeltz+dVz*dt>=dCellLength) { dtTmp=(dCellLength-dDeltz)/dVz; SetDt(dtTmp*1.0e9); }
    if(dDeltz<0) { dBx=0; dBy=0;dBz=0; }

    beam->p.at(iTmp).setE(dEx,dEy,dEz);
    beam->p.at(iTmp).setB(dBx,dBy,dBz);
    beam->p.at(iTmp).particle::moveOneStep();

    if(dDeltz+dVz*dtTmp>=dCellLength)     { SetDt(dt*1.0e9);}
    dDeltz+=(beam->p.at(iTmp).getPosition().getZ()-dZ);
    if(iTmp==1)      OfField<<dZ*100<<" "<<dEz<<" "<<dEz*cos(dPhi)<<" "<<dEx<<" "<<dEx*(cos(dPhi))<<" "<<dPhi*180.0/pi<<" "<<cos(dPhi)<<endl;
    //           if(iTmp==1)
    //           {
    //               double dVx=beam->p.at(iTmp).getVelocity().getX();
    //               double dVy=beam->p.at(iTmp).getVelocity().getY();
    //               OfField<<dZ<<" "<<" "<<dPhi*180.0/pi<<" "<<dErest*(1.0/sqrt(1.0-(dVx*dVx+dVy*dVy+dVz*dVz)/clight/clight)-1.0)<<endl;
    //           }
    }
    }
     */
    //begin-next
    vector<double> dCount(iParticleNumber+1,0);
    dDeltz=0;
    dCellLength/=1.0;
    int flagTmp=1;
    int istep=1;
    cout<<istep<<endl;
    double dEx,dEy,dEz,dBx,dBy,dBz;
    for(int i=1;i<2;i++)
    {
        //getBeam();
        beam->calRms(); 
        dDeltz=beam->getZCenter();
        while(dDeltz<i*dCellLength-dZero)
        {
            getBeam();
            cout<<istep++<<endl;
            for(int iTmp=0;iTmp<=iParticleNumber;iTmp++)
            {
                if(dCount[iTmp]<i*dCellLength)
                {
                    dPhi=dFreq*2*pi*(beam->p.at(iTmp).getT0());
                    dZ=beam->p.at(iTmp).getPosition().getZ();
                    dX=beam->p.at(iTmp).getPosition().getX();
                    dY=beam->p.at(iTmp).getPosition().getY();
                    dVz=beam->p.at(iTmp).getVelocity().getZ();
                    Interpret(dX,dY,dZ,EOut,BOut);
                    dBx=BOut->getX()*(-sin(dPhi));dBy=BOut->getY()*(-sin(dPhi));dBz=BOut->getZ();  //dBz=0
                    dEx=EOut->getX()*(cos(dPhi));dEy=EOut->getY()*(cos(dPhi));dEz=EOut->getZ()*cos(dPhi);
                    beam->p.at(iTmp).setE(dEx,dEy,dEz);
                    beam->p.at(iTmp).setB(dBx,dBy,dBz);
                    if(dCount[iTmp]<dLQuad1) { beam->p.at(iTmp).setE(dBx+dSQuad1*dY,dBy+dSQuad1*dX,dBz); }
                    else if(dCount[iTmp]>dCellLength-dLQuad2) { beam->p.at(iTmp).setE(dBx+dSQuad2*dY,dBy+dSQuad2*dX,dBz); }
                    if(dCount[iTmp]+dVz*dt>=dCellLength*i) { flagTmp=0;dtTmp=(i*dCellLength-dCount[iTmp])/dVz; SetDt(dtTmp*1.0e9); }

                    beam->p.at(iTmp).particle::moveOneStep();
                    //  if(dCount[iTmp]+dVz*dtTmp>=i*dCellLength)     { SetDt(dt*1.0e9);}
                    if(flagTmp==0)     { flagTmp=1;SetDt(dt*1.0e9);}
                    dCount[iTmp]+=(beam->p.at(iTmp).getPosition().getZ()-dZ);
                    //		if(iTmp==1)     OfField<<dZ*100<<" "<<dEx<<" "<<dEy<<" "<<cos(dPhi)<<endl;
                    //if(iTmp==1)   OfField<<dZ*100<<" "<<dEz<<" "<<dEz*cos(dPhi)<<" "<<dEx<<" "<<dEx*(cos(dPhi))<<" "<<dPhi*180.0/pi<<" "<<cos(dPhi)<<endl;
                }
                //if(dCount[iTmp]<dDeltz-dZero) { dDeltz=dCount[iTmp];}
            }
            beam->calRms(); 
            dDeltz=beam->getZCenter();
        }
    }
    //    OfField.close();
    //    beam->p.at(0).setPositionZ(dz+dCellLength);
    delete EOut;
    delete BOut;
# endif
    for(int i=0;i<=iParticleNumber;i++)
    {
        // double dtTmp=beam->p.at(i).getT0();
        // beam->p.at(i).setT0(dtTmp-1.0/dFreq);
        double dzTmp=beam->p.at(i).getPosition().getZ();
        beam->p.at(i).setPositionZ(dzTmp-dCellLength);
    }
    //getBeam();
}
void CTrackMp::DTL_FieldMap(string FM_EMCyl,string QuadMap,double _dCellLength)
{
    //GapPhase(3.7084,-35);
		iParticleNumber=beam->getParticleNumber();
		SetPhaseMove(-217.6);
		Read_FieldMap_EMCyl(FM_EMCyl);
		QuadForFieldMap(QuadMap);
    double dPhi;
    double dDeltz=0;
    double dX,dY,dZ,dVz,dVx,dVy;
    double  dG=0;
    CVector3d *EOut;                    //声明为什么不能挪到.h函数中呢？
    CVector3d *BOut;
    CVector3d *EIn;                    //声明为什么不能挪到.h函数中呢？
    CVector3d *BIn;
    EOut=new CVector3d();
    BOut=new CVector3d();
    //EIn=new CVector3d(0,0,0);
    BIn=new CVector3d(0,0,0);
    double dz0=beam->p.at(0).getPosition().getZ();
    vector<double> dCount(iParticleNumber+1,0);
    dDeltz=0;
    dCellLength=_dCellLength/100.0;
    dCellLength/=1;
    int flagTmp=1;
    int istep=1;
    cout<<istep<<endl;
    int lossflag=0;
    ofstream Of("E-Syn");
    getBeam();
    dDeltz=beam->getZCenter();
    double dvCenter=(beam->getBetaAve())*clight;;
    double dEInx,dEIny,dEInz,dBInx,dBIny,dBInz;
    double dGammaAve=beam->getEnergyAve()/dErest+1.0;
    double dEx,dEy,dEz,dBx,dBy,dBz;
    //double dGammaAve=1.0/sqrt(1.0-dvCenter*dvCenter/clight/clight);
    //space-cahrge
    double dtTmp;
    SetNSC(1);
    SC->InputBeam();
    //begin SC test
		SC->scheff();      
    /*
       ofstream ofSC("SC-field");
       double dtmp=0;
       while (dtmp<0.007)
       {
       EIn=SC->Interpret(dtmp,0,0);
       ofSC<<dtmp<<" "<<EIn->getX()<<" "<<EIn->getY()<<" "<<EIn->getZ()<<" "<<endl;
       dtmp+=0.0001;
       }
       ofSC.close();
     */
    //end SC test
		int QuadFlag=0;
    for(int i=1;i<=1;i++)
    {
        while(dDeltz<i*dCellLength-dZero)
        {
					if(istep%NSC==0) 
					{
						if(CheckBeamSize()==1)
						{
							SC->InputBeam();
						}      //参数dt现在没用了
						SC->scheff();
					}
            for(int iTmp=0;iTmp<=iParticleNumber;iTmp++)
            {
                if(lossflag==1||lossflag==-1)  continue;
                else if(lossflag==0)
									//else if(lossflag==0&&dCount[iTmp]<i*dCellLength)
                {
                    dPhi=dFreq*2*pi*(beam->p.at(iTmp).getT0());
                    dZ=beam->p.at(iTmp).getPosition().getZ();
                    dX=beam->p.at(iTmp).getPosition().getX();
                    dY=beam->p.at(iTmp).getPosition().getY();
                    if(sqrt(dX*dX+dY*dY)>dBoreRadius-dZero) { beam->p.at(iTmp).setLossFlag(1); continue; }
                    dVz=beam->p.at(iTmp).getVelocity().getZ();
                    Interpret(dX,dY,dZ-dZedge,EOut,BOut);
                    dG=InterpretQuad(dZ-dZedge);   //attention
										if(dG!=0) QuadFlag=1;


                    /*
                    double dk,dGamma;
                    dVx=beam->p.at(iTmp).getVelocity().getX();
                    dVy=beam->p.at(iTmp).getVelocity().getY();
                    dGamma=1.0/sqrt(1.0-(dVx*dVx+dVy*dVy+dVz*dVz)/clight/clight);
                    dk=sqrt(1.0-1.0/dGamma/dGamma)*clight*dG/dGamma/dErest/1.0e6*clight*clight*dt;
                    dVx=-(dk)*dX+dVx;
                    dVy=dk*dY+dVy;
                    */
                    //if(istep%NSC==0) EIn=SC->Interpret(dX,dY,dZ);
                    if(istep%NSC==0) EIn=SC->Interpret(dX,dY,dZ-dDeltz);
                    else EIn->setCVector3d(0,0,0);
                    //EIn->setCVector3d(0,0,0);
                    EIn=SC->Interpret(dX,dY,dZ-dDeltz); //dz有问题  //单位是cm
                    dEInx=dGammaAve*EIn->getX();
                    dEIny=dGammaAve*EIn->getY();
                    dEInz=EIn->getZ();
                    dBInx=-dGammaAve*dvCenter*EIn->getY()/clight/clight;
                    dBIny=dGammaAve*dvCenter*EIn->getX()/clight/clight;
                    dBInz=0.0;
										/*
										 * 或者假设vz=vc表示为只有电场的形式
                    dEInx=EIn->getX()/dGammaAve;
                    dEIny=EIn->getY()/dGammaAve;
                    dEInz=EIn->getZ();
										*/

                    dBx=BOut->getX()*(-sin(dPhi))+dG*dY+dBInx*NSC;
                    dBy=BOut->getY()*(-sin(dPhi))+dG*dX+dBIny*NSC;
                    dBz=0;
										/*
										//longitudinal quad field,fringe field
										if(dG>0.1)
										dBz=BOut->getZ()*(-sin(dPhi))-dG*(1.0-dG/50)*4.518219*(dX+dY)+dBInz*NSC;  //dBz=0
										else if(dG<-0.1)
										dBz=BOut->getZ()*(-sin(dPhi))-dG*(1.0+dG/50)*4.518219*(dX+dY)+dBInz*NSC;  //dBz=0
										*/
										//dBz=BOut->getZ()*(-sin(dPhi))+(1.0/dG-1.0)/dG/dG*(dX+dY)+dBInz*NSC;  //dBz=0
                    dEx=abs(chargeOverMass)/chargeOverMass*EOut->getX()*cos(dPhi)+dEInx*NSC;
                    dEy=abs(chargeOverMass)/chargeOverMass*EOut->getY()*cos(dPhi)+dEIny*NSC;
                    dEz=abs(chargeOverMass)/chargeOverMass*EOut->getZ()*cos(dPhi)+dEInz*NSC;
                    beam->p.at(iTmp).setE(dEx,dEy,dEz);
                    beam->p.at(iTmp).setB(dBx,dBy,dBz);
                    //beam->p.at(iTmp).setE(0,0,0);
                    //beam->p.at(iTmp).setB(dG*dY,dG*dX,0);
                    //if(dCount[iTmp]+dVz*dt>=i*dCellLength) { flagTmp=0;dtTmp=(i*dCellLength-dCount[iTmp])/dVz; SetDt(dtTmp*1.0e9); }

                    beam->p.at(iTmp).particle::moveOneStep();
                    //					if(flagTmp==0)     { flagTmp=1;SetDt(dt*1.0e9);}
                    dCount[iTmp]+=(beam->p.at(iTmp).getPosition().getZ()-dZ);
                    //		if(iTmp==1)     OfField<<dZ*100<<" "<<dEx<<" "<<dEy<<" "<<cos(dPhi)<<endl;
                    // if(iTmp==1)   Of<<dZ*100<<" "<<dEz/cos(dPhi)<<" "<<dEz<<" "
                    // 	<<dEx/cos(dPhi)<<" "<<dEx<<" "<<dPhi*180.0/pi<<" "<<cos(dPhi)<<endl;
                    dZ=beam->p.at(iTmp).getPosition().getZ();
                    //					if(iTmp==0)                Of<<dZ*100<<" ";
                    ////					if(iTmp==1)   Of<<dX<<" "<<dY<<" "<<dG*dX<<" "<<dG*dY<<" "<<dG<<" ";
                    ////					if(iTmp==2)   Of<<dX<<" "<<dY<<" "<<dG*dX<<" "<<dG*dY<<" "<<dG<<endl;
                    //					else Of<<dX<<" "<<dY<<" ";
                    //					if(iTmp==16) Of<<endl;
                    if(iTmp==0)   Of<<dZ*100<<" "<<dG<<endl;
                    //if(iTmp==0)   Of<<dZ*100<<" "<<dVz<<" "<<EOut->getZ()<<" "<<dPhi<<endl;
                }
            }
            // beam->calRms();
            cout<<++istep<<endl;
            //			double dzmax,dzmin,dG1,dG2;
            //			dzmax=beam->getZMax();
            //			dzmin=beam->getZMin();
            //			dG1=InterpretQuad(dzmax/100.0);
            //			dG2=InterpretQuad(dzmin/100.0);
            //		  if(dG1==0&&dG2==0)getBeam(); 
            //			else beam->calRms();
						//print the beam every five steps
						if((istep-1)%10==0)
							getBeam(); 
						else beam->calRms(0);
            dDeltz=beam->getZCenter()-dz0;
            //			if(dDeltz>0.18)
            //			OutCordFile(istep);
						QuadFlag=0;
        }
        //getBeam(); 
        Of.close();                
    }
    delete EOut;
    delete BOut;
    //delete EIn;
    delete BIn;
		dZedge+=dCellLength;
}
void CTrackMp::OutCordFile(int _nCell)
{
    nCell=_nCell;
    stringstream ss;
    string sTmp;
    ss<<nCell;
    ss>>sTmp;
    string distrit="dist";
    distrit+=sTmp;
    distrit+=".txt";
    ofstream ofile(distrit.c_str());
    cout<<"开始outcordFile"<<endl;
    if(!ofile)
    {
        cout<<"不能打开束流分布文件"<<endl;
        exit(1);
    }
    double dE;
    double dvx,dvy,dvz,dXTmp,dYTmp,dZTmp;
    double dT0;
    iParticleNumber=beam->getParticleNumber();
    for(int i=0;i<iParticleNumber+1;i++)
    {
        dXTmp=beam->p.at(i).getPosition().getX();
        dYTmp=beam->p.at(i).getPosition().getY();
        dZTmp=beam->p.at(i).getPosition().getZ();
        dvx=beam->p.at(i).getVelocity().getX();
        dvy=beam->p.at(i).getVelocity().getY();
        dvz=beam->p.at(i).getVelocity().getZ();
        dE=dErest/sqrt(1.0-(dvz*dvz+dvx*dvx+dvy*dvy)/clight/clight)-dErest;
        dT0=beam->p.at(i).getT0();
        //		ofile<<dXTmp*100<<" "<<beam->p.at(i).getVelocity().getX()/dvz<<" "
        //			<<dYTmp*100<<" "<<beam->p.at(i).getVelocity().getY()/dvz<<" ";
        ofile<<dXTmp<<" "<<dvx<<" "<<dYTmp<<" "<<dvy<<" "<<dZTmp<<" "<<dvz<<" ";
        ofile<<dT0*2*pi*dFreq<<" ";
        //ofile<<setprecision(10)<<beam->vdPhase.at(i)<<" ";
        ofile<<setprecision(10)<<dE<<"    "<<dZTmp<<endl;
    }
    ofile.close();
}
void CTrackMp::OutBeamRmsSize()
{
    ofstream ofBeam("beam.out");
    ofBeam<<"   Z(cm)    XRms      YRms      ZRms(cm)    PhiRms      ERms       Xmax      Ymax         XAve      "
        <<"YAve        PhiAve       XpRms      YpRms   "
        <<"    XXpRms      YYpRms   dEnergyAve(MeV)  dWmax   dWMin   GoodParNum  ParLossTran"<<endl;
    for(int i=0;i<vvdBeamRms.size();i++)
    {
        for(int j=0;j<vvdBeamRms[i].size();j++)
        {
            ofBeam<<vvdBeamRms[i][j]<<"  ";
        }
        ofBeam<<endl;
    }
		ofBeam.close();
		ofBeam.open("beam-Ave.out");
		for(int i=0;i<vvdBeamRms.size();i++)
		{
			ofBeam<<beam->getAveSize()->getX()<<" ";
			ofBeam<<beam->getAveSize()->getY()<<" ";
			ofBeam<<beam->getAveSize()->getZ()<<" ";
			ofBeam<<beam->getAveSizep()->getX()<<" ";
			ofBeam<<beam->getAveSizep()->getY()<<" ";
			ofBeam<<beam->getAveSizep()->getZ()<<"\n ";
		}
		ofBeam.close();
		ofBeam.open("beam-Rms.out");
		for(int i=0;i<vvdBeamRms.size();i++)
		{
			ofBeam<<beam->getRmsSize()->getX()<<" ";
			ofBeam<<beam->getRmsSize()->getY()<<" ";
			ofBeam<<beam->getRmsSize()->getZ()<<" ";
			ofBeam<<beam->getRmsSizep()->getX()<<" ";
			ofBeam<<beam->getRmsSizep()->getY()<<" ";
			ofBeam<<beam->getRmsSizep()->getZ()<<"\n ";
		}
		ofBeam.close();
}
void CTrackMp::OutTwiss()
{
    ofstream ofBeam("twiss.out");
    ofBeam<<"  Z(cm)      alphx     alphy       alphz        betax       betay  betaz(cm/mrad) emitx     emity  emitz(cm*mrad)"
        <<" emitPhiW(deg*MeV) )emitXY2D(cm*cm) betaS gammaS"<<endl;
    for(int i=0;i<vvdTwissRms.size();i++)
    {
        for(int j=0;j<vvdTwissRms[i].size();j++)
        {
            ofBeam<<setw(8)<<vvdTwissRms[i][j]<<"  ";
        }
        ofBeam<<endl;
    }
    ofBeam.close();
}
void CTrackMp::getBeam()
{
    beam->calRms(1); //dafault 1;1 z-code;0 t-code
    vector<double>  vdTmp;
    vdTmp.resize(0);
    vdTmp.push_back(beam->getZCenter()*100);               //1
		//the next line can not be removed,or it affect the CheckBeamsize function
    vdTmp.push_back(beam->p.at(0).getPosition().getZ());   //2
    vdTmp.push_back(beam->getRmsSize()->getX()*100.0);     //3
    vdTmp.push_back(beam->getRmsSize()->getY()*100.0);     //4
    vdTmp.push_back(beam->getRmsSize()->getZ()*100.0);     //5
    vdTmp.push_back(beam->getERms());                      //6
    vdTmp.push_back(beam->getXMax());                      //7
    vdTmp.push_back(beam->getYMax());                      //8
    vdTmp.push_back(beam->getAveSize()->getX()*100.0);     //9
    vdTmp.push_back(beam->getAveSize()->getY()*100.0);     //10
    vdTmp.push_back(beam->getAveSize()->getZ()*100.0);     //11
    vdTmp.push_back(beam->getAveSizep()->getX()*clight/dErest/1.0e6);          //13
    vdTmp.push_back(beam->getAveSizep()->getY()*clight/dErest/1.0e6);          //14
    vdTmp.push_back(beam->getAveSizep()->getZ()*clight/dErest/1.0e6);          //15
    vdTmp.push_back(beam->getRmsSizep()->getX()*clight/dErest/1.0e6);          //16
    vdTmp.push_back(beam->getRmsSizep()->getY()*clight/dErest/1.0e6);          //17
    vdTmp.push_back(beam->getRmsSizep()->getZ()*clight/dErest/1.0e6);          //18
    //	vdTmp.push_back(beam->getXpRms());                 //
    //	vdTmp.push_back(beam->getYpRms());                 //
    vdTmp.push_back(beam->getXXp());                       //18
    vdTmp.push_back(beam->getYYp());                       //19
    vdTmp.push_back(beam->getZZp());                       //20
    vdTmp.push_back(beam->getEnergyAve());                 //21
    vdTmp.push_back(beam->getEMax());                      //22
    vdTmp.push_back(beam->getEMin());                      //23
    vdTmp.push_back(beam->getGoodParticle());              //24
    vdTmp.push_back(beam->getParticleLossT());             //25
    vvdBeamRms.push_back(vdTmp);
    vdTmp.resize(0);
    //vdTmp.push_back(beam->p.at(0).getPosition().getZ());
    vdTmp.push_back(beam->getZCenter()*100);
    vdTmp.push_back((*beam->getAlph())[0]);
    vdTmp.push_back((*beam->getAlph())[1]);
    vdTmp.push_back((*beam->getAlph())[2]);
    vdTmp.push_back((*beam->getBeta())[0]);
    vdTmp.push_back((*beam->getBeta())[1]);
    vdTmp.push_back((*beam->getBeta())[2]);
    vdTmp.push_back((*beam->getRMSEmittance())[0]);
    vdTmp.push_back((*beam->getRMSEmittance())[1]);
    vdTmp.push_back((*beam->getRMSEmittance())[2]);
    vdTmp.push_back(beam->getEmitRmsPhiW());
    vdTmp.push_back(beam->getEmitRmsXY2D());
    vdTmp.push_back(beam->getBetaAve());
//    double dZ1=beam->getZCenter();
//    double dG1=InterpretQuad(dZ1);
//    vdTmp.push_back(dG1);
    //vdTmp.push_back(beam->getEnergyAve());
    vdTmp.push_back((beam->getEnergyAve())/dErest+1.0);
    vdTmp.push_back(beam->getEmitRms4D());
    vdTmp.push_back(beam->getEmitRms6D());
    // vdTmp.push_back(beam->getBetaS());
    // vdTmp.push_back(beam->getEnergyS()/dErest+1.0);
    vvdTwissRms.push_back(vdTmp);
}
void CTrackMp::Read_FieldMap_EMCyl(string _FieldMap)
{
//可以改成直接用superfish输出文件的形式，参考convert.cpp
    ifstream infile(_FieldMap.c_str());
    if(!infile)
    {
        cout<<"can't open fieldmap file"<<endl;
        return;
    }
    double x1,x2,x3,x4,x5,x6;
    string sRow="";
    stringstream ss;
    int iFMapSize=0;
    vector<double>          vdFMap;
    vector<vector<double> > vvdFMap;
    vvdFMap.resize(0);
    double dTmpOld,dTmpNew;
    vvvdFMap.resize(0);
    while(sRow=="")
    {
        //输入文件格式原因，电场分布文件中不要有空行
        //输入文件单位是cm，内部单位是m
        getline(infile,sRow);        
        if(sRow==""||sRow=="\r"||sRow==" \r") continue;
        ss.clear();
        ss.str(sRow);        
        ss>>drMin>>drMax>>NrMap>>dzMin>>dzMax>>NzMap;
        drMax=drMax/100.0; drMin=drMin/100.0;
        dzMax=dzMax/100.0; dzMin=dzMin/100.0;
        drMap=(drMax-drMin)/double(NrMap-1);
        dzMap=(dzMax-dzMin)/double(NzMap-1);
        break ;
    }
    for(int i=0;i<NrMap;i++)
    {
        for(int j=0;j<NzMap;j++)
        {
            vdFMap.resize(0);
            getline(infile,sRow);
            if(sRow=="") continue;
            ss.clear();
            ss.str(sRow);        
            ss>>x1>>x2>>x3;
            vdFMap.push_back(x2*1.0e6); //er in V/m
            vdFMap.push_back(x1*1.0e6); //ez in V/m
            //vdFMap.push_back(x3); //B
            vdFMap.push_back(x3*4.0e-7*pi); //B
            vvdFMap.push_back(vdFMap);
        }
        vvvdFMap.push_back(vvdFMap);
        for(int i=0;i<vvdFMap.size();i++)
					vvdFMap[i].resize(0);
        vvdFMap.resize(0);
    }
    if(infile.eof())    //要求数据最后没有空行？
    {
        cout<<"the field map grid doesn't agree with the first line"<<endl;
    }
    infile.close();
		CVector3d *EOut;
		CVector3d *BOut;
		EOut=new CVector3d();
		BOut=new CVector3d();
		ofstream of("e-in-axies");
		double testz=0;
		//for(int i=0;i<vvvdFMap[0].size();i++)
		//of<<vvvdFMap[20][i][1]<<" "<<vvvdFMap[20][i][0]<<endl;
		//while(testz<0.148732)
		while(testz<7.741591)
		{
			Interpret(0,0.0,testz,EOut,BOut);
			testz+=5.0e-4;
			of<<testz<<" "<<EOut->getX()<<" "<<EOut->getY()<<" "<<EOut->getZ()<<" "<<BOut->getX()<<endl;
		}
		delete EOut;
		delete BOut;
		of.close();
} 
void CTrackMp::MapForTW(string str)
    //change the fieldmap file from superfish to the standard form for tracewin 2D cylindrical frame
{
    //FieldMap(str); 
		Read_FieldMap_EMCyl(str);
    string file1,file2,file3;
    file1=str+".edr";
    file2=str+".edz";
    file3=str+".bdq";
    ofstream ofer(file1.c_str());
    ofstream ofez(file2.c_str());
    ofstream ofbq(file3.c_str());
    ofer<<NzMap-1<<" "<<dzMax<<"\n"<<NrMap-1<<" "<<drMax<<"\n"<<"1"<<endl;
    ofez<<NzMap-1<<" "<<dzMax<<"\n"<<NrMap-1<<" "<<drMax<<"\n"<<"1"<<endl;
    ofbq<<NzMap-1<<" "<<dzMax<<"\n"<<NrMap-1<<" "<<drMax<<"\n"<<"1"<<endl;
    for(int i=0;i<NzMap;i++)
        for(int j=0;j<NrMap;j++)
        {
            ofer<<vvvdFMap[j][i][0]*1.0e-6<<endl;
            ofez<<vvvdFMap[j][i][1]*1.0e-6<<endl;
            ofbq<<vvvdFMap[j][i][2]<<endl;
        }
    ofer.close();
    ofez.close();
    ofbq.close();
}
void CTrackMp::Interpret(double dX,double dY, double dZ,CVector3d *EOut,CVector3d *BOut)
{
    //用于等距离的外场格点
    //为了统一，长度都用m作为单位
    double dR=sqrt(dX*dX+dY*dY);
    int ir=int((dR-drMin)/drMap);
    int iz=int((dZ-dzMin)/dzMap);
    double dEr,dEx,dEy,dEz,dBPhi;
    double dBx,dBy,dBz;
    double drRatio,dzRatio;
    double dPhi;
    if(dZ-dzMin<-dzMap||dZ-dzMax>dzMap)  {dEr=0;dEz=0;dBPhi=0;}
    else if(dR>drMax)  {dEr=0;dEz=0;dBPhi=0;}
    else if(dZ-dzMin<-dZero&&dZ-dzMin>-dzMap)
    {
        drRatio=(dR-drMap*ir-drMin)/drMap;
        dEr=(dZ-(dzMin-dzMap)/dzMap)*((vvvdFMap[ir][0][0])*(1.0-drRatio)+vvvdFMap[ir+1][0][0]*drRatio);
        dEz=(dZ-(dzMin-dzMap)/dzMap)*((vvvdFMap[ir][0][1])*(1.0-drRatio)+vvvdFMap[ir+1][0][1]*drRatio);
        dBPhi=(dZ-(dzMin-dzMap)/dzMap)*((vvvdFMap[ir][0][2])*(1.0-drRatio)+vvvdFMap[ir+1][0][2]*drRatio);
    }
    else if(dZ-dzMax>dZero&&dZ-dzMax<dzMap)
    {
        drRatio=(dR-drMap*ir-drMin)/drMap;
        dEr=((dZ-dzMax)/dzMap)*((vvvdFMap[ir][NzMap-1][0])*(1.0-drRatio)+vvvdFMap[ir+1][NzMap-1][0]*drRatio);
        dEz=((dZ-dzMax)/dzMap)*((vvvdFMap[ir][NzMap-1][1])*(1.0-drRatio)+vvvdFMap[ir+1][NzMap-1][1]*drRatio);
        dBPhi=((dZ-dzMax)/dzMap)*((vvvdFMap[ir][NzMap-1][2])*(1.0-drRatio)+vvvdFMap[ir+1][NzMap-1][2]*drRatio);
    }
    else {
        dzRatio=(dZ-dzMap*iz-dzMin)/dzMap;
        drRatio=(dR-drMap*ir-drMin)/drMap;

        dEr=vvvdFMap[ir][iz+1][0]*dzRatio*(1.0-drRatio)+vvvdFMap[ir][iz][0]*(1.0-dzRatio)*(1.0-drRatio)
            +vvvdFMap[ir+1][iz][0]*(1.0-dzRatio)*drRatio+vvvdFMap[ir+1][iz+1][0]*drRatio*dzRatio;
        dEz=vvvdFMap[ir][iz+1][1]*dzRatio*(1.0-drRatio)+vvvdFMap[ir][iz][1]*(1.0-dzRatio)*(1.0-drRatio)
            +vvvdFMap[ir+1][iz][1]*(1.0-dzRatio)*drRatio+vvvdFMap[ir+1][iz+1][1]*drRatio*dzRatio;
        dBPhi=vvvdFMap[ir][iz+1][2]*dzRatio*(1.0-drRatio)+vvvdFMap[ir][iz][2]*(1.0-dzRatio)*(1.0-drRatio)
            +vvvdFMap[ir+1][iz][2]*(1.0-dzRatio)*drRatio+vvvdFMap[ir+1][iz+1][2]*drRatio*dzRatio;
    }
    if(fabs(dX)<dZero) {dEy=dEr;dEx=0;dBx=-dBPhi;}
    else
    {
        dPhi=atan(dY/dX);
        if(dX<0)  dPhi=dPhi+pi;
        dEx=dEr*cos(dPhi);
        dEy=dEr*sin(dPhi);
        dBx=-dBPhi*sin(dPhi);
        dBy=dBPhi*cos(dPhi);
        dBz=0;
    }
    EOut->setCVector3d(dEx,dEy,dEz);
    BOut->setCVector3d(dBx,dBy,dBz);
}
void CTrackMp::GapPhase(double _dL,double _dPhiS)
    //已知入口绝对相位，和gap中心的设计相位，得到每个粒子应该的phasechange
    //应该判断，cell1才进行这个函数
{
    cout<<"begin GapPhase change"<<endl;
    double dL=_dL/100.0;
    double dPhiIn=dFreq*360.0*(beam->p.at(0).getT0());
    double dPhiShift=0;
    CVector3d *EOut;
    CVector3d *BOut;
    EOut=new CVector3d();
    BOut=new CVector3d();
    double dPhi=_dPhiS+10.0; //有问题
    //double dPhi=_dPhiS*180/pi+1.0; //有问题
    double dPhiIn2=dPhiIn;
    double dDeltz=0;
    double dVz,dVz0,dZ0;
    double dEz;
    dVz0=beam->p.at(0).getVelocity().getZ();
    dZ0=beam->p.at(0).getPosition().getZ();
    while(fabs(dPhi-_dPhiS)>dZero)
    {
        dDeltz=0;
        while(dDeltz-dL<dZero)
        {
            Interpret(0,0,dZ,EOut,BOut);
            //相位怎么办？
            dZ=beam->p.at(0).getPosition().getZ();
            dPhi=2*pi*dFreq*(beam->p.at(0).getT0());
            dEz=EOut->getZ()*cos(dPhi);
            beam->p.at(0).setE(0,0,dEz);
            beam->p.at(0).setB(0,0,0);
            dVz=beam->p.at(0).getVelocity().getZ();
            if(dDeltz+dVz*dt>=dL) { SetDt((dL-dDeltz+dZero)/dVz*1.0e9); }
            beam->p.at(0).particle::moveOneStep();
            dDeltz+=(beam->p.at(0).getPosition().getZ()-dZ);
            if(dDeltz+dVz*dt>=dL) { SetDt(dt*1.0e9);}
            dZ=beam->p.at(0).getPosition().getZ();
            dPhi=2*pi*dFreq*(beam->p.at(0).getT0());
            //    cout<<dPhi*180/pi<<" "<<dDeltz<<endl;
        }
        dPhi=dFreq*360.0*(beam->p.at(0).getT0());
        dPhiShift=_dPhiS-dPhi;
        dPhiIn2=dPhiIn2+dPhiShift;
        beam->p.at(0).setT0((dPhiIn2)/360.0/dFreq);
        beam->p.at(0).setPositionZ(dZ0);
        beam->p.at(0).setVelocityZ(dVz0);
        cout<<dPhiIn2-dPhiIn<<" "<<dZ<<endl;
    }
    for(int i=1;i<=iParticleNumber;i++)
    {
        dPhi=beam->p.at(i).getT0()*360*dFreq;
        beam->p.at(i).setT0((dPhi+dPhiIn2-dPhiIn)/360.0/dFreq);
    }
    delete EOut;
    delete BOut;
}
void  CTrackMp::SearchInitPhase()
{
    double   dVz0,dZ0;
    dVz0=beam->p.at(0).getVelocity().getZ();
    dZ0=beam->p.at(0).getPosition().getZ();
    double d1,d2,d3;
    d1=0.0;d3=2*pi;
    //d2=(d1+d3)/2.0; 
    d2=d1+5.0*pi/180.0;
    vector<double> vdVOut;
    ofstream of("vdVOut.txt");
    double  dV1,dV2,dV3;
    while(d2<d3)
    {
        beam->p.at(0).setT0((d2)/2.0/pi/dFreq);
        SParticleTrack();
        dV1=beam->p.at(0).getVelocity().getZ();
        vdVOut.push_back(dV1);
        of<<d2/pi*180<<" "<<dV1<<endl;
        d2=d2+5.0*pi/180.0;
        beam->p.at(0).setPositionZ(dZ0);
        beam->p.at(0).setVelocityZ(dVz0);
    }
    of.close();
}
void  CTrackMp::SParticleTrack()
{
    CVector3d *EOut;
    CVector3d *BOut;
    EOut=new CVector3d();
    BOut=new CVector3d();
    double   dDeltz=0;
    double   dVz;
    double   dPhi,dEz;
    dDeltz=0;
    while(dDeltz<dCellLength)
    {
        Interpret(0,0,dZ,EOut,BOut);
        dZ=beam->p.at(0).getPosition().getZ();
        dPhi=2*pi*dFreq*(beam->p.at(0).getT0());
        dEz=EOut->getZ()*cos(dPhi);
        beam->p.at(0).setE(0,0,dEz);
        beam->p.at(0).setB(0,0,0);
        dVz=beam->p.at(0).getVelocity().getZ();
        if(dDeltz+dVz*dt>=dCellLength) { SetDt((dCellLength-dDeltz+dZero)/dVz*1.0e9); }
        beam->p.at(0).particle::moveOneStep();
        dDeltz+=(beam->p.at(0).getPosition().getZ()-dZ);
        if(dDeltz+dVz*dt>=dCellLength) { SetDt(dt*1.0e9);}
        //    cout<<dPhi*180/pi<<" "<<dDeltz<<endl;
        //   只要比较vz就可以，横向速度等于0
    }

    delete EOut;
    delete BOut;
}
void CTrackMp::QuadForFieldMap(string _QuadFile)
{
    ifstream infile(_QuadFile.c_str());
    if(!infile)
    {
        cout<<"can't open Quadmap file"<<endl;
        return;
    }
    string sRow;
    stringstream ss;
    /*
       getline(infile,sRow);        
       ss.clear();
       ss.str(sRow);        
       ss>>dzQuadMin>>dzQuadMax>>NzQuadMap;
       dzQuadMap=(dzQuadMax-dzQuadMin)/(NzQuadMap-1);
       double dtmp;
       vdQuadMap.resize(0);
       for(int i=0;i<NzQuadMap;i++)
       {
       getline(infile,sRow);        
       ss.clear();
       ss.str(sRow);        
       ss>>dtmp;
       vdQuadMap.push_back(dtmp);
       }
     */
    int iQuadNum;
    getline(infile,sRow);        
    ss.clear();
    ss.str(sRow);        
    ss>>iQuadNum;//数目scale
    vector<double> vdtmp;
    double  dtmp;
    vvdQuadMap.resize(0);
    for(int i=0;i<iQuadNum;i++)
    {
        vdtmp.resize(0);
        getline(infile,sRow);        
        ss.clear();
        ss.str(sRow);        
        while(ss>>dtmp)
            vdtmp.push_back(dtmp);
        vvdQuadMap.push_back(vdtmp);
    }
    //test Quad gradient
    dtmp=0;
    double QuadStr;
    double dX=0.005;
    ofstream of1("Quad-padsc");
    while(dtmp<0.60)
    {
        QuadStr=InterpretQuad(dtmp);
        of1<<dtmp*100<<" "<<dX<<" "<<QuadStr<<" "<<QuadStr*dX<<endl;
        dtmp+=0.001;
    }
    of1.close();
    //end test Quad gradient
    /*
    //test firnge field function
    ofstream of1("fringe-Quad");
    double dFr;
    double y;
    dtmp=-20;
    while(dtmp<20)
    {
    //	  if(dtmp<0) dFr=FringeQuad(-dtmp-10);
    //		else dFr=FringeQuad(dtmp-10);
    //	  of1<<dtmp<<" "<<dFr<<endl;
    //		dtmp+=0.01;
    dFr=FringeQuad(5,4,dtmp);
    of1<<dtmp<<" "<<dFr<<endl;
    dtmp+=0.01;
    }
    of1.close();
     */
    /* for TraceWin and InterpretQuad test*/
    /*
       dtmp=0;
       ofstream of1("OUTSF7.bsx");
       ofstream of2("OUTSF7.bsy");
       ofstream of3("OUTSF7.bsz");
       double QuadStr;
       int stepz=0;
       while(dtmp<100.01)
    //while(dtmp<(vvdQuadMap[iQuadNum-1][1]+vvdQuadMap[iQuadNum-1][0]))
    {
    for(int j=-10;j<11;j++)   //y
    for(int k=-10;k<11;k++)  //x
    {
    QuadStr=InterpretQuad(dtmp/100.0);
    of1<<QuadStr*j*0.008/10.0<<endl;    //Bx=G*y
    of2<<QuadStr*k*0.008/10.0<<endl;    //By=G*x
    of3<<dtmp<<" "<<0.0<<endl;
    }
    dtmp+=0.1;
    stepz+=1;
    }
    cout<<"dtmp "<<dtmp<<" stepz "<<stepz<<endl;

    of1.close();
    of2.close();
    of3.close();
     */
}
double CTrackMp::FringeQuad(double dCenter,double dL,double dz)
    //_dz单位是cm
{
    /*
       double dv1,dv2;
       double dr1,dr2;
       double _dz;
       if(dz<dCenter) _dz=-dz+dCenter-(dL/2.0);
       else           _dz= dz-dCenter-(dL/2.0);
       dr1=dBoreRadius*100; dr2=14.0;  //磁铁外径等于14cm
       dv1=1.0/sqrt(1.0+(_dz/dr1)*(_dz/dr1));
       dv2=1.0/sqrt(1.0+(_dz/dr2)*(_dz/dr2));
       double dA=dv1*dv1*dv2*dv2*(dv1*dv1+dv2*dv2+dv1*dv2+4+8/dv1/dv2)/(dv1+dv2);
       dA=0.5*(1-_dz/8.0*(1.0/dr1+1.0/dr2)*dA);
       return dA;
     */

    double c1,c2;
    c1=-0.00004;c2=4.518219;
    //	 double ds=(dz)/dBoreRadius/100.0/2;
    double ds;
    if(dz>dCenter) ds=(dz-dCenter-dL/2.0-0.75*0.8/2.0)/dBoreRadius/100.0/2.0;
    else ds=(-dz+dCenter-dL/2.0-0.75*0.8/2.0)/dBoreRadius/100.0/2.0;
    double dA=1.0/(exp(c1+c2*ds)+1.0);
    return dA;
}
double CTrackMp::InterpretQuad(double dz)
{
    double _dz=dz*100.0;
    int    vvdsize=vvdQuadMap.size()-1;
    double vdsize;
    double dQuadG=0;
    double d1=vvdQuadMap[vvdsize][1]+vvdQuadMap[vvdsize][0];
    double d2=vvdQuadMap[0][0];
    //	double dzExtent=5*(beam->getRmsSize()->getZ())*100;
       double dzExtent=dZero*100;
       if((_dz>d1-dZero)||(_dz<d2+dzExtent-dZero)) return dQuadG;
       else if(_dz>vvdQuadMap[vvdsize][0]-dZero&&_dz<d1) dQuadG=vvdQuadMap[vvdsize][2];
       else if(_dz>d1-dZero&&_dz<d1+dzExtent+dZero) 
       dQuadG=vvdQuadMap[vvdsize][2]-vvdQuadMap[vvdsize][2]/dzExtent*(_dz-d1);
       else if(_dz>vvdQuadMap[vvdsize][0]-dzExtent&&_dz<vvdQuadMap[vvdsize][0]) 
       dQuadG=vvdQuadMap[vvdsize][2]/dzExtent*(_dz-(vvdQuadMap[vvdsize][0]-dzExtent));
       else
       {
       for(int i=0;i<vvdsize;i++)
       {
       if(_dz>vvdQuadMap[i][0]-dzExtent-dZero&&_dz<vvdQuadMap[i+1][0]-dzExtent+dZero)
       {
       if(_dz<vvdQuadMap[i][1]+vvdQuadMap[i][0]+dZero) {dQuadG=vvdQuadMap[i][2]; break;}
       else if(_dz>vvdQuadMap[i][0]-dzExtent-dZero&&_dz<vvdQuadMap[i][0]+dZero)
       { dQuadG=vvdQuadMap[i][2]/dzExtent*(_dz-(vvdQuadMap[i][0]-dzExtent)); break;}
       else if(_dz>vvdQuadMap[i][0]+vvdQuadMap[i][1]-dZero
       &&_dz<vvdQuadMap[i][0]+vvdQuadMap[i][1]+dzExtent+dZero)
       { dQuadG=vvdQuadMap[i][2]+(0-vvdQuadMap[i][2])/dzExtent*(_dz-vvdQuadMap[i][0]-vvdQuadMap[i][1]);
       break;}
       }
       }
       }
       return dQuadG;
    /*
		//if the fringe field is included, the calculation velocity will be slowed down.
    double dFz=0;
    double dCenter=0;
    for(int i=0;i<=vvdsize;i++)
    {
        dCenter=vvdQuadMap[i][1]/2.0+vvdQuadMap[i][0];
        dFz=FringeQuad(dCenter,vvdQuadMap[i][1],_dz);
        dQuadG+=(vvdQuadMap[i][2]*dFz);
    }
    return dQuadG;
     */
}
void CTrackMp::SetBoreRadius(double _dBoreRadius) { dBoreRadius=_dBoreRadius/100.0;}
int CTrackMp::CheckBeamSize()
{
    int iSize=vvdBeamRms.size();
    if((iSize==1)||iOld==0) { iOld=1; return 1;}
    double dx1,dy1,dz1,dx2,dy2,dz2;
    dx2=vvdBeamRms[iSize-1][2];
    dy2=vvdBeamRms[iSize-1][3];
    dz2=vvdBeamRms[iSize-1][4];
    dx1=vvdBeamRms[iOld-1][2];
    dy1=vvdBeamRms[iOld-1][3];
    dz1=vvdBeamRms[iOld-1][4];
    if(dx2/dx1>RemeshFlag||dy2/dy1>RemeshFlag||dz2/dz1>RemeshFlag) {iOld=iSize-1;return 1;}
    else return 0;
}
void CTrackMp::DTL_Fourier(string filename,double dCellLength)
{
    //gsl_test();
    //read the file in which field in axies was written
    //the first line is the total Length
    ifstream ifile(filename.c_str()); 
    ofstream ofile;
    if(!ifile)
    {
        cout<<"can't open "<<filename<<" file"<<endl;
        return;
    }
    string sRow="";
    stringstream ss;
    double dL,x1;
    vector<double>  vdInAxies;
    getline(ifile,sRow);        
    ss.clear();
    ss.str(sRow);        
    ss>>dL;
    while(getline(ifile,sRow))
    {
        if(sRow=="") continue;
        ss.clear();
        ss.str(sRow);        
        ss>>x1;
        vdInAxies.push_back(x1*1.0e6); //er in V/m
    }
    ifile.close();

    //fourier transform
    int size=vdInAxies.size();

    gsl_fft_complex_wavetable *compwave;
    gsl_fft_complex_workspace *compwork;
    compwork = gsl_fft_complex_workspace_alloc (size);
    compwave = gsl_fft_complex_wavetable_alloc (size);

    double aTmp[2*size];

    for(int k=0;k<size;k++)
    {
        REAL(aTmp,k)=vdInAxies[k];
        IMAG(aTmp,k)=0;
    }
    gsl_fft_complex_forward(aTmp,1,size,compwave,compwork);
    double dk[size];
    double dtmp=0;
		double dk_max=1;
		//double dk_max=size/2*(2*pi/dL);
    for(int k=0;k<size;k++)
    {
        if(k<size/2)        //it matters nothing whether there is equal signal or not
            dk[k]=k*(2*pi/dL)/dk_max;
        else dk[k]=(k-size)*(2*pi/dL)/dk_max;
    }

    //gradient for Er
    double bTmp[2*size];
    for(int i=0;i<size;i++)
    {
        bTmp[2*i]=  aTmp[2*i]*dk[i]*dk[i];
        bTmp[2*i+1]=aTmp[2*i+1]*dk[i]*dk[i];
    }

    for(int i=0;i<size;i++)
    {
        dtmp=aTmp[2*i];
        aTmp[2*i]=-aTmp[2*i+1]*dk[i];
        aTmp[2*i+1]=dtmp*dk[i];
    }
    //aTmp[size+1]=0;
    //ofile.open("gradient");
    //for(int i=0;i<size;i++)
    //ofile<<i*dL/size<<" "<<bTmp[2*i]<<" "<<bTmp[2*i+1]<<endl;
    //ofile.close();
    //ofile.open("gradient");
		//for(int i=1;i<size;i++)
		//{
		//   ofile<<i*dL/size<<" "<<vdInAxies[i]<<" "<<aTmp[2*i]<<" "<<aTmp[2*i+1]<<" " <<" "<<bTmp[2*i]<<
		//			 " "<<bTmp[2*i+1]<<" "<<aTmp[2*i+1]+aTmp[2*(size-i)+1]<<" "<<bTmp[2*i+1]+bTmp[2*(size-i)+1]<<endl;
		//}

    gsl_fft_complex_inverse(aTmp,1,size,compwave,compwork);
    gsl_fft_complex_inverse(bTmp,1,size,compwave,compwork);

    gsl_fft_complex_workspace_free(compwork);
    gsl_fft_complex_wavetable_free(compwave);

		/*
    dZ=0;
    double step=1;
    if(size>dZero) step=dL/size;
    double dEz,dEx,dEy,dEz2,dBx,dBy,dBz;
    int j=0;
		double dR=0.002;
		dX=0.002;dY=0;
    while(dZ<0.07)
      //   while(dZ<1)
    {
      j=int(dZ/step);
			dEz=vdInAxies[j]+(vdInAxies[j+1]-vdInAxies[j])/step*(dZ-step*j);
      ofile<<dZ<<" "<<dEz<<" ";
			dEz2=bTmp[2*j]+(bTmp[2*(j+1)]-bTmp[2*j])/step*(dZ-step*j);
			dEz2=-0.25*dR*dR*dEz2-dR*dR*dR/6.0/clight/clight*dEz;
			dEz=dEz+dEz2;
			dEx=-(aTmp[2*j]+(aTmp[2*(j+1)]-aTmp[2*j])/step*(dZ-step*j))*0.5*dX;
			dEy=-(aTmp[2*j]+(aTmp[2*(j+1)]-aTmp[2*j])/step*(dZ-step*j))*0.5*dY;
			dBx=(vdInAxies[j]+(vdInAxies[j+1]-vdInAxies[j])/step*(dZ-step*j))/2.0/clight/clight*dX;
			dBy=(vdInAxies[j]+(vdInAxies[j+1]-vdInAxies[j])/step*(dZ-step*j))/2.0/clight/clight*dY;
      dZ+=0.00014;
      ofile<<dEz2<<" "<<dEx<<" "<<dEy<<" "<<dBx<<" "<<dBy<<endl;
    }
		*/
		//ofile.close();
  /*
    double aTmp[size];
    gsl_fft_real_wavetable        *realwave;
    gsl_fft_halfcomplex_wavetable *hcwave;
    gsl_fft_real_workspace        *realwork;
    realwork = gsl_fft_real_workspace_alloc (size);
    realwave = gsl_fft_real_wavetable_alloc (size);

    for(int k=0;k<size;k++) { aTmp[k]=vdInAxies[k]; }
    gsl_fft_real_transform(aTmp,1,size,realwave,realwork);
    double dk[size];
    for(int k=0;k<size;k++)
    {
        if(k<size/2)
            dk[k]=k*(2*pi/dL);
        else dk[k]=(k-size)*(2*pi/dL);
    }
    for(int i=0;i<size;i++) { aTmp[i]=aTmp[i]*dk[i]; }
    hcwave = gsl_fft_halfcomplex_wavetable_alloc (size);
    gsl_fft_halfcomplex_inverse(aTmp,1,size,hcwave,realwork);

    ofstream of("gradient");
    for(int i=0;i<size;i++)
    of<<aTmp[i]<<endl;
    of.close();


    gsl_fft_real_workspace_free (realwork);
    gsl_fft_real_wavetable_free (realwave);
    gsl_fft_halfcomplex_wavetable_free (hcwave);
    */
    double dX,dY,dZ,dR,dPhi;
    double dEz,dEx,dEy,dBx,dBy,dBz;
    int j=0;
    double step=1.0;
    dBz=0;
    if(size>dZero) step=dL/size;
    double dDeltz=0 ;
    int istep=0;
    double dEz2=0;
    ofile.open("field");
		double dG=0;
    while(dDeltz<dCellLength-dZero)
    {
        for(int iTmp=0;iTmp<=iParticleNumber;iTmp++)
        {
            dPhi=dFreq*2*pi*(beam->p.at(iTmp).getT0());
            dX=beam->p.at(iTmp).getPosition().getX();
            dY=beam->p.at(iTmp).getPosition().getY();
            dZ=beam->p.at(iTmp).getPosition().getZ();
            dR=sqrt(dX*dX+dY*dY);   //?
						dG=InterpretQuad(dZ);
            if(dZ<dZero||dZ>dL)       {dEx=0;dEy=0;dEz=0;dBx=0;dBy=0;dBz=0;}
            else
            {
                j=int(dZ/step);
                dEz=(vdInAxies[j]+(vdInAxies[j+1]-vdInAxies[j])/step*(dZ-step*j))*cos(dPhi);
                dEz2=(bTmp[2*j]+(bTmp[2*(j+1)]-bTmp[2*j])/step*(dZ-step*j))*dk_max*dk_max*cos(dPhi);
                dEz2=-0.25*dR*dR*dEz2-dR*dR*dR/6.0/clight/clight*dEz;
                //dEz=dEz+dEz2;
                dEx=-(aTmp[2*j]+(aTmp[2*(j+1)]-aTmp[2*j])/step*(dZ-step*j))*dk_max*0.5*dX*cos(dPhi);
                dEy=-(aTmp[2*j]+(aTmp[2*(j+1)]-aTmp[2*j])/step*(dZ-step*j))*dk_max*0.5*dY*cos(dPhi);
                dBx=(vdInAxies[j]+(vdInAxies[j+1]-vdInAxies[j])/step*(dZ-step*j))/2.0/clight/clight*dX*(-sin(dPhi));
                dBy=(vdInAxies[j]+(vdInAxies[j+1]-vdInAxies[j])/step*(dZ-step*j))/2.0/clight/clight*dY*(-sin(dPhi));
								dBz=0;
								if(iTmp==0) 
								{
								ofile<<dZ<<" "<<dEz<<" ";
								ofile<<-(aTmp[2*j]+(aTmp[2*(j+1)]-aTmp[2*j])/step*(dZ-step*j))*dk_max*0.5*0.001<<" ";
                ofile<<(vdInAxies[j]+(vdInAxies[j+1]-vdInAxies[j])/step*(dZ-step*j))/2.0/clight/clight*0.001<<endl;
								}
            }
						dBx+=dG*dY;
						dBy+=dG*dX;

            beam->p.at(iTmp).setE(dEx,dEy,dEz);
            beam->p.at(iTmp).setB(dBx,dBy,dBz);
            beam->p.at(iTmp).particle::moveOneStep();
        }
        ++istep;
        cout<<istep<<endl;
        getBeam(); 
        dDeltz=beam->getZCenter();
    }
}
void CTrackMp::gsl_test()
{
    int size=1000;
    double cTmp[2*size];
    for(int k=0;k<size;k++)
    {REAL(cTmp,k)=k;
    IMAG(cTmp,k)=0; }
    gsl_fft_complex_wavetable *compwave;
    gsl_fft_complex_workspace *compwork;
    compwork = gsl_fft_complex_workspace_alloc (size);
    compwave = gsl_fft_complex_wavetable_alloc (size);

    ofstream of("test");
    for(int i=0;i<size;i++)
    of<<cTmp[2*i]<<" "<<cTmp[2*i+1]<<endl;
    of.close();

    gsl_fft_complex_forward(cTmp,size/100,size,compwave,compwork);
    gsl_fft_complex_inverse(cTmp,size/100,size,compwave,compwork);

    gsl_fft_complex_workspace_free(compwork);
    gsl_fft_complex_wavetable_free(compwave);
}
vector<double> CTrackMp::getMatrixInput_Twiss()
{
	 vector<double> vdTwiss;
	 for(int i=1;i<=9;i++)
	 vdTwiss.push_back(vvdTwissRms[0][i]);
	 return vdTwiss;
}
vector<double> CTrackMp::getMatrixInput_Beam()
{
	 vector<double> vdTwiss;
	 for(int i=0;i<3;i++)
	 vdTwiss.push_back((*beam->getRmsSize())[i]);
	 for(int i=0;i<3;i++)
	 vdTwiss.push_back((*beam->getRmsSizep())[i]);
	 return vdTwiss;
}
void CTrackMp::testParticle()
{
	ofstream of("Single_Particle");
	beam->p.at(1).setPosition(0,0,0);
	beam->p.at(1).setVelocity(1.0e5,1.0e5,0);
	beam->p.at(1).setT0(0);
	double max=0;
	double min=0;
	double dx,dy;
	for(int i=0;i<1000;i++)
	{
		of<<beam->p.at(1).getT0()<<" ";
		of<<beam->p.at(1).getPosition().getX()<<" "
		  <<beam->p.at(1).getPosition().getY()<<" "
		  <<beam->p.at(1).getPosition().getZ()<<" "
		  <<beam->p.at(1).getVelocity().getX()<<" "
		  <<beam->p.at(1).getVelocity().getY()<<" "
		  <<beam->p.at(1).getVelocity().getZ()<<endl;
		beam->p.at(1).setE(0,0,0);
		beam->p.at(1).setB(0,0,1);
		beam->p.at(1).particle::moveOneStep();
		dx=beam->p.at(1).getPosition().getY();
		if(max<dx)  max=dx;
		if(min>dx)  min=dx;
	}
	of.close();
	cout<<max<<" "<<min<<" "<<endl;
}
